Performance  Evaluation  of  Temporal  Range 
Registration  for  Unmanned  Vehicle  Navigation 

R.  Madhavan  and  E.  Messina 

Intelligent  Systems  Division 
National  Institute  of  Standards  and  Technology 
Gaithersburg,  MD  20899-8230. 

Tel:  (301)  975-2865  Fax:  (301)  990-9688 
Email:  {raj  .madhavan,  elena.messina}  @nist.gov 


ABSTRACT  —  In  this  paper,  we  evaluate  the  performance 
of  an  iterative  registration  algorithm  for  position  estimation  of 
Unmanned  Ground  Vehicles  (UGVs)  operating  in  unstructured 
environments.  Field  data  obtained  from  trials  on  UGVs  traversing 
undulating  outdoor  terrain  is  used  to  quantify  the  performance  of 
the  algorithm  in  producing  continual  position  estimates.  These 
estimates  are  then  compared  with  those  provided  by  ground 
truth  to  facilitate  the  performance  evaluation  of  the  algorithm. 
Additionally,  we  propose  performance  measures  for  assessing  the 
quality  of  correspondences.  These  measures,  collectively,  provide 
an  indication  of  the  quality  of  the  correspondences  thus  making 
the  registration  algorithm  more  robust  to  outliers  as  spurious 
matches  are  not  used  in  computing  the  incremental  transformation. 
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1.  INTRODUCTION 

Active  range  sensing  has  become  an  integral  part  of 
any  unmanned  vehicle  navigation  system  due  its  ability  to 
produce  unambiguous,  direct,  robust,  and  precise  images 
consisting  of  range  pixels.  This  is  in  direct  contrast  to  pas¬ 
sive  sensing  where  the  inference  of  range  largely  remains 
computationally  intensive  and  not  robust  enough  for  use 
in  natural  outdoor  environments.  Depending  on  the  speed 
of  the  vehicle,  operating  environment,  and  data  rate,  such 
range  images  acquired  from  a  moving  platform  need  to  be 
registered  to  make  efficient  use  of  information  contained  in 
them  for  various  navigation  tasks  including  map-building, 
localization,  obstacle  avoidance,  and  control. 

Iconic  methods  that  attempt  to  minimize  the  discrepan¬ 
cies  between  sensed  data  and  a  model  of  the  environment 
have  been  utilized  for  range  registration.  The  attraction  of 
these  methods  lies  in  the  fact  that  the  matching  works 
directly  on  data  points.  Because  the  search  is  confined  to 
small  perturbations  of  the  range  images,  it  is  computation¬ 
ally  efficient.  For  example,  Kanade  et  al.  [3]  compared  el¬ 
evation  maps  obtained  from  3D  range  images  to  determine 
vehicle  location.  A  similar  iconic  approach  has  also  been 
adopted  by  Shaffer  [10]  but  it  does  not  take  into  account 
the  uncertainty  associated  with  the  observation  data. 


We  have  developed  a  temporal  iterative  algorithm  for 
registering  range  images  obtained  from  unmanned  vehicles. 
Formally,  the  process  of  registration  is  defined  as  follows: 
Given  two  sets  of  range  images  (model  set:  M  and  data  set: 
D),  find  a  transformation  (rotation  and  translation)  which 
when  applied  to  D  minimizes  a  distance  measure  between 
the  two  point  sets.  Despite  the  apparent  simplicity  of  the 
problem,  to  register  range  images  from  unmanned  vehicles 
traversing  unstructured  environments,  the  terrain  of  travel, 
sensor  noise  and  determination  of  accurate  correspondences 
make  it  quite  challenging. 

The  registration  algorithm  is  a  modified  variant  of  the 
well-known  Iterative  Closest  Point  (ICP)  algorithm  [1].  At 
each  iteration,  the  algorithm  determines  the  closest  match 
for  each  point  and  updates  the  estimated  position  based  on 
a  least-squares  metric  with  some  modifications  to  increase 
robustness.  The  modified  algorithm  has  been  shown  to  be 
robust  to  outliers  and  false  matches  during  the  registration 
of  3D  range  images  obtained  from  a  scanning  LADAR 
rangefinder  on  an  Unmanned  Ground  Vehicle  (UGV)  and 
also  towards  registering  LADAR  images  from  the  UGV 
with  those  from  an  Unmanned  Aerial  Vehicle  (UAV)  that 
flies  over  the  terrain  being  traversed  [9].  For  completeness, 
the  temporal  iterative  registration  algorithm  is  summarized 
in  Section  2. 

In  this  paper,  we  evaluate  the  performance  of  the  regis¬ 
tration  algorithm  for  position  estimation  of  UGVs  operating 
in  unstructured  environments.  Field  data  obtained  from  two 
trials  on  UGVs  traversing  undulating  outdoor  terrain  is  used 
to  quantify  the  performance  of  the  algorithm  in  producing 
continual  position  estimates.  Using  the  data  obtained  from 
the  first  trial,  the  iterative  registration  algorithm  aids  the  po¬ 
sition  estimation  process  whenever  Global  Positioning  Sys¬ 
tem  (GPS)  estimates  are  unavailable  or  are  below  required 
accuracy  bounds.  In  the  second  trial,  ICP  is  combined  with 
a  post-correspondence  Extended  Kalman  Filter  (EKF)  to 
account  for  uncertainty  present  in  the  range  images.  For 
both  the  trials,  the  position  estimates  are  then  compared 
with  those  provided  by  ground  truth  to  facilitate  the  perfor¬ 
mance  evaluation  of  the  registration  algorithm.  In  addition. 
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we  propose  performance  measures  for  assessing  the  quality 
of  correspondences.  These  measures,  collectively,  provide 
an  indication  of  the  quality  of  the  correspondences  thus 
making  the  registration  algorithm  more  robust  to  outliers  as 
spurious  matches  are  not  used  in  computing  the  incremental 
transformation.  The  registration  algorithm  is  then  combined 
with  proposed  performance  metrics  and  compared  to  the 
traditional  ICP  algorithm  in  terms  of  accuracy  and  speed. 

The  paper  is  structured  as  below:  Section  2  describes  the 
iterative  temporal  registration  algorithm.  Section  3  presents 
experimental  results  when  the  iterative  algorithm  is  used 
for  obtaining  position  estimates.  Section  3.1  compares 
registration-aided  position  estimates  with  those  provided 
by  GPS.  Section  3.2  details  a  map-aided  registration  algo¬ 
rithm  for  pose  estimation.  Section  4  develops  performance 
measures  for  quality  assessment  of  correspondences  within 
the  registration  process  and  provides  the  associated  results. 
Section  5  provides  the  conclusions  and  outlines  areas  of 
continuing  research. 

2.  ITERATIVE  TEMPORAL  REGISTRA¬ 
TION  ALGORITHM 

The  process  of  registration  is  stated  formally  as: 

mm(R,T)y]||Mi-(RDi  +  T)||2  (1) 

where  R  is  the  rotation  matrix,  T  is  the  translation  vector 
and  the  subscript  i  refers  to  the  corresponding  points  of  the 
sets  M  and  D. 

2.7.  Iterative  Closest  Point  Algorithm 

The  ICP  algorithm  can  be  summarized  as  follows:  Given 
an  initial  motion  transformation  between  the  two  point 
sets,  a  set  of  correspondences  are  developed  between  data 
points  in  one  set  and  the  next.  For  each  point  in  the  first 
data  set,  find  the  point  in  the  second  that  is  closest  to  it 
under  the  current  transformation.  It  should  be  noted  that 
correspondences  between  the  two  point  sets  is  initially 
unknown  and  that  point  correspondences  provided  by  sets 
of  closest  points  is  a  reasonable  approximation  to  the  true 
point  correspondence.  From  the  set  of  correspondences,  an 
incremental  motion  can  be  computed  facilitating  further 
alignment  of  the  data  points  in  one  set  to  the  other.  This  find 
correspondence/compute  motion  process  is  iterated  until  a 
predetermined  threshold  termination  condition. 

In  its  simplest  form,  the  ICP  algorithm  can  be  described 
by  the  following  steps: 

1.  For  each  point  in  data  set  D,  compute  its  closest  point 
in  data  set  M.  In  this  paper,  this  is  accomplished  via 
nearest  point  search  from  the  set  comprising  A^d  data 
and  A^m  model  points. 

2.  Compute  the  incremental  transformation  (R,T)  using 
Singular  Value  Decomposition  (SVD)  via  correspon¬ 
dences  obtained  in  step  1. 


3.  Apply  the  incremental  transformation  from  step  2.  to 

D. 

4.  If  relative  changes  in  R  and  T  are  less  than  a  threshold, 
terminate.  Else  go  to  step  1. 

To  deal  with  spurious  points/false  matches  and  to  account 
for  occlusions  and  outliers,  we  modify  and  weight  the  least- 
squares  objective  function  in  Equation  (1)  such  that  [11]: 

™^(R.T)  ||Mj  -  (RDj  +  T)  11^  (2) 

i 

If  the  Euclidean  distance  between  a  point  Xi  in  one  set 
and  its  closest  point  yi  in  the  other,  denoted  by  di  = 
d{xi,yi),  is  bigger  than  the  maximum  tolerable  distance 
threshold  V^ax^  then  Wi  is  set  to  zero  in  Equation  (2). 
This  means  that  an  Xi  cannot  be  paired  with  a  yi  since 
the  distance  between  reasonable  pairs  cannot  be  very  big. 
The  value  of  V^ax  is  set  adaptively  in  a  robust  manner  by 
analyzing  distance  statistics. 

Let  {xi^yi^di}  be  the  set  of  original  points,  the  set  of 
closest  points  and  their  distances,  respectively.  The  mean 
and  standard  deviation  of  the  distances  are  computed  as: 


where  N  is  the  total  number  of  pairs. 

The  pseudo-code  for  the  Adaptive  Thresholding  (AT)  of 
the  distance  V^ax  is  given  below: 
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where  itn  denotes  the  iteration  number  and  D  is  a  function 
of  the  resolution  of  the  range  data. 

During  implementation,  V  was  selected  based  on  the 
following  two  observations: 

1)  If  V  is  too  small,  then  several  iterations  are  re¬ 
quired  for  the  algorithm  to  converge  and  several  good 
matches  will  be  discarded,  and 

2)  If  V  is  too  big,  then  the  algorithm  may  not  converge  at 
all  since  many  spurious  matches  will  be  included.  The 
interested  reader  is  referred  to  [1 1]  for  more  details  on 
the  effect  and  selection  of  V  and  e  on  the  convergence 
of  the  algorithm. 

At  the  end  of  this  step,  two  corresponding  point  sets, 
PM-{Pi}  and  PD-{qi}  are  available. 

The  incremental  transformation  (rotation  and  translation) 
of  step  2.  is  obtained  as  follows: 

.  Calculate  -  Pc)(qi  -  Qc)^;  (PcQc)  are 

the  centroids  of  the  point  sets  (Pm,Pd)- 


•  Find  the  Singular  Value  Decomposition  (SVD)  of  H 

such  that  H  =  where  U  and  V  are  unitary 

matrices  whose  columns  are  the  singular  vectors  and 
ri  is  a  diagonal  matrix  containing  the  singular  values. 

•  The  rotation  matrix  relating  the  two  point  sets  is  given 
by  R  =  VU^. 

•  The  translation  between  the  two  point  sets  is  given  by 

T  =  Qc  -  Rpc. 

This  process  is  iterated  as  stated  in  step  4.  until  the 
mean  Euclidean  distance  between  the  corresponding  point 
sets  Pm  and  Pd  is  less  than  or  equal  to  a  predetermined 
distance  or  until  a  given  number  of  iterations  is  exceeded. 

3.  EXPERIMENTAL  RESULTS 

3.1,  Registration-aided  Position  Estimation 

In  this  section,  we  estimate  the  position  of  an  UGV  oper¬ 
ating  in  an  unknown  outdoor  environment.  The  registration 
algorithm  is  used  for  aiding  position  estimation  whenever 
GPS  errors  are  above  a  predetermined  threshold^. 

An  Extended  Kalman  Filter  (EKF)  was  used  to  fuse 
encoder,  GPS  and  compass  observations  to  arrive  at  ground 
truth  position  estimates.  It  should  be  noted  here  that  the 
EKF  pose  estimate  is  always  superior  than  that  provided  by 
GPS  alone  and  thus  has  been  considered  as  ground  truth. 
Consequently,  a  better  position  fix  is  guaranteed  even  when 
GPS  is  subject  to  multipathing  errors.  The  ground  truth  was 
obtained  in  a  similar  fashion  as  reported  in  [8]. 

Figure  1  shows  the  results  of  the  position  estimation 
using  the  registration  algorithm.  As  mentioned  earlier,  reg¬ 
istration  of  range  images  is  used  to  aid  position  estimation 
when  GPS  reported  positional  errors  exceed  a  given  thresh¬ 
old.  In  Figure  1(a),  the  registration-aided  position  estimates 
are  denoted  by  and  that  of  the  GPS  by  ‘o’.  The  wheel 
encoder  estimates  are  also  shown  by  ‘x’  for  comparison. 
The  error  between  the  GPS  and  the  registration-aided 
position  estimates  as  compared  with  the  ground  truth  are 
shown  in  Figure  1(b).  It  is  evident  that  the  registration-aided 
estimates  are  far  superior  than  that  of  GPS  alone. 

3.2,  2D  Map- aided  Position  Estimation 

A  map-aided  position  estimation  algorithm  for  comput¬ 
ing  accurate  pose  estimates  for  a  UGV  operating  in  tunnel¬ 
like  environments  is  described  in  this  section.  Using  ground 
truth^  together  with  the  information  from  a  range  and 
bearing  scanning  laser  rangefinder,  a  map  of  the  operating 
domain,  represented  by  a  polyline  that  adequately  approx¬ 
imates  the  geometry  of  the  environment,  is  obtained.  The 
map  building  process  relies  on  position  estimation  provided 
by  artificial  landmarks. 

Uhe  error  in  the  GPS  positions  reported  were  obtained  as  a  function  of 
the  number  of  satellites  acquired.  As  an  alternative,  the  so-called  dilution 
of  precision  measure  associated  with  the  GPS  can  be  used  for  the  same 
purpose  [2]. 

^The  ground  truth  was  obtained  using  a  rotating  laser  scanner  and 
known  artificial  landmarks  placed  at  surveyed  locations  [7]. 


An  Iterative  Closest  Point-Extended  Kalman  Eilter  (ICP- 
EKE)  algorithm  is  used  to  match  range  images  from  a  scan¬ 
ning  laser  rangefinder  to  the  line  segments  of  the  polyline 
map  [6].  Eor  this  application,  ICP  alone  does  not  provide 
sufficiently  reliable  and  accurate  vehicle  motion  estimates. 
These  shortcomings  are  overcome  by  combining  the  ICP 
with  a  post-correspondence  EKE.  Once  correspondences  are 
established,  a  post-correspondence  EKE,  with  the  aid  of  a 
non-linear  observation  model,  provides  consistent  vehicle 
pose  estimates. 

The  ICP-EKE  algorithm  has  several  advantages.  Eirst, 
the  uncertainty  associated  with  observations  is  explicitly 
taken  into  account.  Second,  observations  from  a  variety  of 
different  sensors  can  be  easily  combined  as  the  changes  are 
refiected  only  as  additional  observational  states  in  the  EKE. 
Third,  laser  observations  that  do  not  correspond  to  any  line 
segment  of  the  polyline  map  are  discarded  during  the  EKE 
update  stage  thus  making  the  algorithm  robust  to  errors  in 
the  map. 

The  estimated  vehicle  positions  (solid  line)^  provided  by 
the  ICP-EKE  algorithm  along  with  the  ground  truth  (dotted 
line)  is  shown  in  Eigure  2(a).  The  vehicle  travels  a  distance 
of  150  meters  from  right  to  left.  The  corresponding  2cr 
confidence  bounds  for  the  absolute  error  in  x,  y  and  <p 
are  shown  in  Eigure  2(b).  It  can  be  seen  that  the  errors 
are  bounded  and  thus  the  pose  estimates  are  consistent.  It 
is  also  clear  that  the  estimated  path  is  in  close  agreement 
with  the  ground  truth. 

4.  PERFORMANCE  MEASURES 

The  correspondence  determination  process  is  the  most 
challenging  step  of  the  iterative  algorithm.  Establishing 
reliable  correspondences  is  extremely  difficult  as  the  UGV 
is  subjected  to  heavy  pitching  and  rolling  motion  charac¬ 
teristic  of  travel  over  undulating  terrain.  This  is  further 
exacerbated  by  the  uncertainty  of  the  location  of  the  sensor 
platform  relative  to  the  global  frame  of  reference.  In  ad¬ 
dition  to  these  factors,  noise  inherently  present  in  range 
images  complicates  the  process  of  determining  reliable 
correspondences. 

One  solution  to  overcome  the  above  deficiencies  is 
to  extract  naturally  occurring  view-invariant  features,  for 
example,  corners,  from  range  images.  Such  ground  control 
points  can  then  be  used  for  establishing  reliable  registration 
with  the  ICP  algorithm  converging  to  the  global  minimum. 
A  hybrid  approach  to  register  aerial  images  obtained  from 
a  UAV  with  those  from  the  UGV  was  developed  by  aug¬ 
menting  the  modified  ICP  algorithm  with  a  feature-based 
method.  The  feature-based  hybrid  approach  was  shown  to 
be  effective  in  producing  reliable  registration  for  UGV 
navigation  [9]. 

Eor  the  map-aided  position  estimation  scheme  described 
in  Section  3.2,  the  ICP-EKE  algorithm  failed  to  produce 

^As  the  estimates  and  the  their  corresponding  ground  truth  are  very 
close,  extra  effort  is  required  on  the  part  of  the  reader  to  distinguish 
between  the  two. 
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Fig.  1.  Registration-aided  position  estimation.  The  aided  estimates  are  shown  by  ‘+’  and  that  of  GPS  by  ‘o’.  The  wheel  encoder  estimates  shown  by 
‘x’  are  included  for  comparison.  In  (b),  position  errors  as  compared  to  the  ground  truth  is  depicted  (GPS  estimate  is  shown  in  dashed-dotted  line). 


unambiguous  correspondences  with  the  map  whenever  vari¬ 
ations  in  data  sets  were  not  unique.  To  enable  ICP  to  pro¬ 
duce  accurate  correspondences,  a  strategy  to  augment  the 
ICP-EKF  algorithm  with  artificial/natural  landmarks  was 
devised  to  provide  external  aiding.  To  facilitate  the  selection 
of  landmarks,  an  entropy-based  metric  was  developed  to 
enable  the  evaluation  of  information  contained  in  a  potential 
landmark.  A  curvature  scale  space  algorithm  was  developed 
to  extract  natural  landmarks  from  laser  rangefinder  scans 
[5].  The  proposed  landmark  augmentation  methodology  has 


been  verified  for  the  localization  of  a  Load-Haul-Dump 
truck  and  resulted  in  the  ICP-EKF  algorithm  producing 
reliable  and  consistent  estimates  [6]. 

We  propose  the  following  measures  towards  performance 
evaluation  of  the  registration  algorithm  for  position  estima¬ 
tion. 

4.1,  ICP  Estimate  and  Dead-reckoning  Prediction  Measure 

The  ICP  itself  can  be  used  to  compute  the  estimates  of 
the  pose  of  the  UGV.  This  can  be  compared  with  dead- 
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Fig.  2.  2D  Map-aided  Position  Estimation.  ICP-EKF  estimated  position 
the  trial  vehicle  (solid  line)  and  the  ground  truth  (dotted  line)  are  shown  in 
(a).  The  la  confidence  bounds  are  computed  using  the  covariance  estimate 
for  the  error  m  x,  y  and  (f)  compared  to  the  actual  error  computed  with 
the  ground  truth  estimates  as  depicted  in  (b). 


reckoning  estimates  each  time  before  the  correspondences 
are  computed.  More  specifically,  the  prediction  covariance 
(from  dead-reckoning)  can  be  utilized  as  a  check  on  the  ICP 
estimates,  since  if  the  associated  state  covariances  become 
large,  this  is  an  indication  of  the  state  estimation  filter 
divergence  as  a  result  of  the  poor  ICP  estimates. 

The  largest  Eigen  value  of  the  predicted  state  covariance 
matrix  (that  is  a  measure  of  the  total  positional  uncertainty) 
can  be  used  as  a  measure  to  check  the  quality  of  the  ICP 
estimates. 

Also  the  determinant  of  the  predicted  state  covariance 
matrix  can  be  used  as  a  measure  since  it  represents  total 


predicted  uncertainty  and  this  can  be  observed  to  see  if  the 
ICP  produces  reliable  and  non-divergent  estimates  (since 
once  the  ICP  estimates  start  behaving  erratically,  this  is 
refiected  by  similar  behavior  in  the  correspondences). 


42.  Mean  Squared  Error  Measure 

To  indicate  if  the  correspondences  make  sense  the  fol¬ 
lowing  measure  is  proposed: 

Vmse  = 

i=l 

where  pi  and  £i  are  the  of  n  range  data  points  and 
d  {pi,£i)  is  the  distance  from  the  p\^  point  to  the  point. 
Global  minimum  of  the  function  will  occur  at  the  true  pose 
of  the  vehicle. 

At  the  true  pose,  all  or  at  least  the  majority  of  the  range 
data  points  will  be  close  to  their  corresponding  points,  thus 
yielding  a  very  low  value  for  the  correct  solution.  The 
problem  with  this  measure  is  that  it  is  difficult  to  decide  if 
the  pose  is  true  in  the  presence  of  outliers  and  occlusions. 


4.3.  Classification  Factor 

Similar  to  [4],  we  define  well  defined  data  points  as  those 
points  that  lie  within  some  distance  threshold  from  their 
corresponding  points: 

d^ 

d^  ^c^ 

where  d  =  d{pi,ii),  c  =  neighborhood  size,  m  =  sigmoid 
steepness"^.  At  true  pose,  global  maximum  should  approach 
close  to  unity  and  will  be  less  in  the  neighborhood  of  well 
defined  data  points.  Note  that  Vcf  values  can  fall  only 
between  [0,1]. 

Indirectly  this  measure  indicates  the  future-goodness  of 
the  pose  estimate  if  a  certain  threshold  is  exceeded.  The 
problem  with  this  measure  could  be  that  it  is  not  as 
sensitive  as  Vmse  since  it  applies  only  for  a  certain  local 
neighborhood.  Thus  Vmse  can  be  used  as  a  comparative 
performance  measure  whereas  Vcf  for  pass/fail  decisions 
for  the  correspondences  before  they  are  passed  on  for 
computing  the  incremental  transformation. 


4.4.  Comparative  Performance  Measure 
It  is  the  ratio  defined  by 


V, 


epm 


V mse 


The  peak  of  this  measure  should  occur  at  the  true  pose. 
In  other  words,  this  measure  serves  as  a  nonlinear  scaling 
factor  applied  to  the  inverse  of  the  measure,  Vmse- 


sigmoid  function  is  given  by  f{a)  =  where  g  denotes 

gain. 


4,5,  Results  and  Discussion 

In  this  section,  we  use  3D  LADAR  data  obtained  during 
field  trials  to  illustrate  the  utility  of  the  proposed  metrics 
in  assessing  the  quality  of  correspondences.  The  LADAR 
was  mounted  on  a  UGV  traversing  rugged  terrain  on  a 
pan/tilt  platform  to  increase  its  narrow  20°  field  of  view. 
The  range  of  the  tilt  motion  is  ±30°  resulting  in  an  effective 
field  of  view  of  about  90°  and  providing  a  range  image 
of  32  lines  x  180  pixels  where  each  data  point  contains 
the  distance  to  a  target  in  the  operating  environment.  The 
angular  resolution  of  this  LADAR  is  0.658°  x  0.5°  in  the 
horizontal  and  vertical  directions,  respectively. 

We  illustrate  the  combined  utility  of  adaptive  threshold¬ 
ing  and  the  Vmse  measure  by  using  it  to  register  3D  range 
images.  We  then  compare  the  registration  results  with  direct 
ICP  (i.e.,  without  AT  and  Vmse)-  For  the  comparison,  the 
same  termination  threshold  condition  is  employed  for  both 
the  algorithms. 

Figures  3  and  4  summarize  the  comparative  results. 
Figures  3(a)  and  (b)  show  the  registered  LADAR  images 
via  the  direct  and  combined  ICP  algorithms,  respectively. 
The  combined  ICP  needed  39  iterations  whereas  the  direct 
ICP  took  82  iterations.  The  mean  distances  before  and  after 
registration  were  0.19  m  and  0.07  m  for  the  two  algorithms, 
respectively.  Figures  4(a)  and  (b)  show  the  closest  point 
distance  before  and  after  registration.  It  is  thus  evident 
that  the  combined  ICP  algorithm  is  vastly  superior  than 
the  direct  ICP  algorithm  both  in  terms  of  accuracy  and 
speed.  Even  though  the  Vmse  metric  is  sensitive  to  outliers, 
we  contend  that  the  adaptive  thresholding  methodology  in 
combination  with  the  mean-squared  error  metric  provides 
an  acceptable  means  in  inferring  the  validity  of  the  position 
estimate. 

5.  CONCLUSIONS  AND  FURTHER  WORK 

The  evaluation  of  performance  of  an  iterative  registration 
algorithm  for  position  estimation  of  UGVs  operating  in 
unstructured  environments  was  the  main  theme  of  this 
paper.  The  modified  ICP  algorithm  was  used  to  aid  the 
position  estimation  process  and  the  resulting  estimates  were 
compared  with  ground  truth  to  facilitate  the  performance 
evaluation  for  two  sets  of  field  data.  Field  data  obtained 
from  trials  on  UGVs  traversing  undulating  outdoor  terrain 
was  used  to  quantify  the  performance  of  the  algorithm  in 
producing  continual  position  estimates. 

In  the  first  set  of  experimental  results,  registration-aided 
position  estimates  were  generated  whenever  GPS  estimates 
were  unavailable  or  unreliable.  For  the  second  set  of  trials, 
the  ICP-EKF  algorithm  was  used  for  map-aided  position 
estimation.  In  both  cases,  the  presented  results  demon¬ 
strated  the  efficacy  of  the  registration  algorithm  for  position 
estimation. 

The  second  part  of  the  paper  developed  performance 
measures  towards  assessing  the  quality  of  correspondences 


required  for  accurate  and  efficient  registration.  The  modi¬ 
fied  algorithm  was  combined  with  the  mean- squared  error 
metric  to  register  3D  LADAR  range  images.  The  combined 
algorithm  was  then  evaluated  against  the  direct  ICP  algo¬ 
rithm.  The  accompanying  results  showed  the  superiority 
of  the  combined  algorithm  both  in  terms  of  speed  and 
accuracy. 

Future  work  includes  combining  the  measures  to  achieve 
efficient  3D  registration  for  mapping  and  position  esti¬ 
mation  tasks,  both  in  indoor  and  outdoor  environments. 
Currently,  we  are  in  the  process  of  obtaining  LADAR  data 
in  areas  where  GPS  accuracy  degrades  and  then  approaches 
its  best  estimate.  Such  data  sets  would  be  of  immense  value 
in  evaluating  the  utility  of  the  registration  algorithm  and  the 
proposed  performance  measures. 
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(b)  Modified  ICP  with  AT  and  Vmse 
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Fig.  3.  Illustration  of  3D  LADAR  registration  via  the  direct  (w/o  AT  and  Vmse)  and  combined  ICP  algorithms.  The  model  (‘o’)  and  data  (‘+’)  points 
after  registration  are  shown. 
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Fig.  4.  The  registered  (shown  in  dashed-dotted  line)  and  unregistered  closest  point  distances  are  shown  corresponding  to  the  registration  of  range 
images  depicted  in  Figures  3(a)  and  3(b),  respectively. 


